import sympy
cf, cr, lf, lr, m, g = sympy.symbols("cf cr lf lr m g")
# theta1, theta2, theta3, theta4, theta5 = sympy.symbols(
#    "theta1 theta2 theta3 theta4 theta5")
speed, k_phi, d_phi, iz, ix, h = sympy.symbols("speed k_phi d_phi iz ix h")
theta1 = cf+cr
theta2 = lf*cf+lr*cr
theta3 = lf*lf*cf+lr*lr*cr
theta4 = 1/m+h*h/ix
theta5 = -k_phi*h/ix-g
state_matrix = sympy.Matrix([[theta1*theta4/speed, theta2*theta4/speed-speed, -h*d_phi/ix, theta5, 0, 0],
                             [theta2/iz/speed, theta3/iz/speed, 0, 0, 0, 0],
                             [h*theta1/ix/speed, h*theta2/ix /
                              speed, -d_phi/ix, -k_phi/ix, 0, 0],
                             [0, 0, 1, 0, 0, 0],
                             [1, 0, 0, 0, 0, speed],
                             [0, 1, 0, 0, 0, 0]])
control_matrix = sympy.Matrix([[-cf*theta4],
                               [-lf*cf/iz],
                               [-h*cf/ix],
                               [0],
                               [0],
                               [0]])
distur_matrix = sympy.Matrix([[h*k_phi/ix, 0],
                              [0, 0],
                              [k_phi/ix, 0],
                              [0, 0],
                              [0, 0],
                              [0, -speed]])
out_state_matrix = sympy.Matrix([[0, 0, 0, 0, 1, 0]])
zero_matrix = sympy.Matrix([[0]])
expand_matrix = sympy.Matrix([[state_matrix, control_matrix],
                              [out_state_matrix, zero_matrix]])
distur_phi, distur_kappa, y_ref = sympy.symbols(
    "distur_phi distur_kappa y_ref")
distur = sympy.Matrix([[distur_phi],
                       [distur_kappa]])
b_vecotr = sympy.Matrix([[distur_matrix*distur], [y_ref]])
# sympy.init_printing(use_latex='mathjax')
print(sympy.simplify(expand_matrix**(-1))*b_vecotr)
